using System;
using System.Collections.Generic;
using System.Text;

namespace GPSCore
{
    /// <summary>
    /// Represents a position on Earth marked by latitude, longitude, and altitude.
    /// </summary>
    public class Position : ICloneable, IComparable<Position>, IComparer<Position> , IEquatable<Position>
    {
        #region members

        /// <summary>
        /// Latitude 
        /// </summary>
        protected Latitude _latitude;
        
        /// <summary>
        /// Longitude
        /// </summary>
        protected Longitude _longitude;

        /// <summary>
        /// Altitude on the level of sea - is a nullable double so if it is not setted it will be null
        /// </summary>
        protected Nullable<double> _altitude;

        #endregion

        #region constructors

        /// <summary>
        /// Base Constructor
        /// </summary>
        public Position()
        {
            _latitude = new Latitude();
            _longitude = new Longitude();
        }

    
        /// <summary>
        /// Construct a Position Object
        /// </summary>
        /// <param name="Latitude">N/S</param>
        /// <param name="Longitude">E/W</param>
        public Position(Latitude Latitude, Longitude Longitude)
        {
            _longitude = Longitude;
            _latitude = Latitude;
        }



        /// <summary>
        /// Construct a Position Object
        /// </summary>
        /// <param name="Latitude"></param>
        /// <param name="Longitude"></param>
        public Position(double Latitude, double Longitude)
        {
            _latitude = new Latitude(Latitude);
            _longitude = new Longitude(Longitude);
        }
        

        
        /// <summary>
        /// Construct a Position Object
        /// </summary>
        /// <param name="Latitude"></param>
        /// <param name="Longitude"></param>
        /// <param name="Elevation"></param>
        public Position(Latitude Latitude, Longitude Longitude, double Elevation)
        {
            _longitude = Longitude;
            _latitude = Latitude;
            _altitude = Elevation;
        }



        /// <summary>
        /// Construct a Position Object
        /// </summary>
        /// <param name="Latitude"></param>
        /// <param name="Longitude"></param>
        /// <param name="Elevation"></param>
        public Position(double Latitude, double Longitude, double Elevation)
        {
            _latitude = new Latitude(Latitude);
            _longitude = new Longitude(Longitude);
            _altitude = Elevation;
        }


        #endregion

        #region proprierties

        /// <summary>
        /// Longitude
        /// </summary>
        public Longitude Longitude
        {
            get { return _longitude; }
            set { _longitude = value; }
        }

        /// <summary>
        /// Latitude
        /// </summary>
        public Latitude Latitude
        {
            get { return _latitude; }
            set { _latitude = value; }
        }

        /// <summary>
        /// Altitude (Meters)
        /// </summary>
        public Nullable<double> Altitude
        {
            get { return _altitude; }
            set { _altitude = value; }
        }


        #endregion

        #region public Methods
                
        /// <summary>
        /// Great circle distance in Radians to provided Position (double)
        /// </summary>
        /// <param name="Position"></param>
        /// <returns></returns>
        public double GreatCircleDistanceTo(Position Position)
        {
            return GreatCircleDistanceTo(Position.Latitude, Position.Longitude);
        }
        
        /// <summary>
        /// Great circle distance in Radians to provided Latitude and Longitude
        /// </summary>
        /// <param name="Latitude"></param>
        /// <param name="Longitude"></param>
        /// <returns></returns>
        public double GreatCircleDistanceTo(Latitude Latitude, Longitude Longitude)
        {
            /*
            The Haversine formula according to Dr. Math.
            http://mathforum.org/library/drmath/view/51879.html
                
            dlon = lon2 - lon1
            dlat = lat2 - lat1
            a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
            c = 2 * atan2(sqrt(a), sqrt(1-a)) 
            
                
            Where
            * dlon is the change in longitude
            * dlat is the change in latitude
            * c is the great circle distance in Radians.
           
              
            * The locations of the two points in 
            spherical coordinates (longitude and 
            latitude) are lon1,lat1 and lon2, lat2.
            */


            // convert latitude and longitude to radians
            double dLat1InRad = GPSConvertManager.DegreesToRadians( this._latitude.DecimalDegrees );
            double dLong1InRad = GPSConvertManager.DegreesToRadians(this._longitude.DecimalDegrees );
            double dLat2InRad = GPSConvertManager.DegreesToRadians(Latitude.DecimalDegrees);
            double dLong2InRad = GPSConvertManager.DegreesToRadians(Longitude.DecimalDegrees);

            // compute latitude and longitude differences
            double dLongitude = dLong2InRad - dLong1InRad;
            double dLatitude = dLat2InRad - dLat1InRad;


            // Intermediate result a.
            double a = Math.Pow(Math.Sin(dLatitude / 2.0), 2.0) +
                       Math.Cos(dLat1InRad) * Math.Cos(dLat2InRad) *
                       Math.Pow(Math.Sin(dLongitude / 2.0), 2.0);

            // Intermediate result c (great circle distance in Radians).
            double c = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0 - a));

            return c;       
        }

        #endregion


        #region ICloneable Members

        /// <summary>
        /// Clode the Position
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            return new Position(this._latitude.DecimalDegrees, this._longitude.DecimalDegrees);            
        }

        #endregion

        #region IComparable<Position> Members

        int IComparable<Position>.CompareTo(Position other)
        {
            int latComp;

            if (this._latitude.DecimalDegrees > other._latitude.DecimalDegrees)
            {
                latComp = 1;
            }
            else
            {
                if (this._latitude.DecimalDegrees < other._latitude.DecimalDegrees)
                {
                    latComp = -1;
                }
                else
                {
                    latComp = 0;
                }
            }


            int longComp;

            if (this._latitude.DecimalDegrees > other._latitude.DecimalDegrees)
            {
                longComp = 1;
            }
            else
            {
                if (this._latitude.DecimalDegrees < other._latitude.DecimalDegrees)
                {
                    longComp = -1;
                }
                else
                {
                    longComp = 0;
                }
            }

            if ((latComp + longComp) > 0)
            {
                return 1;
            }
            else
            {
                if ((latComp + longComp) < 0)
                {
                    return -1;
                }
                else
                {
                    return 0;
                }
            }

        }

        #endregion

        #region IComparer<Position> Members

        int IComparer<Position>.Compare(Position x, Position y)
        {
            int latComp;

            if (x._latitude.DecimalDegrees > y._latitude.DecimalDegrees)
            {
                latComp = 1;
            }
            else
            {
                if (x._latitude.DecimalDegrees < y._latitude.DecimalDegrees)
                {
                    latComp = -1;
                }
                else
                {
                    latComp = 0;
                }
            }


            int longComp;

            if (x._latitude.DecimalDegrees > y._latitude.DecimalDegrees)
            {
                longComp = 1;
            }
            else
            {
                if (x._latitude.DecimalDegrees < y._latitude.DecimalDegrees)
                {
                    longComp = -1;
                }
                else
                {
                    longComp = 0;
                }
            }

            if ((latComp + longComp) > 0)
            {
                return 1;
            }
            else
            {
                if ((latComp + longComp) < 0)
                {
                    return -1;
                }
                else
                {
                    return 0;
                }
            }


        }

        #endregion

        #region IEquatable<Position> Members

        /// <summary>
        /// Check Latitude and Longitude
        /// </summary>
        /// <param name="other"></param>
        /// <returns></returns>
        public bool Equals(Position other)
        {
            if ((this._latitude.Equals(other._latitude)) & (this._longitude.Equals(other._longitude)))
                return true;
            else
                return false;
        }

        #endregion

        #region operators

        /// <summary>
        /// Sum a certain Distance to a Position (fully Azimuth and Elevation.
        /// </summary>
        /// <param name="Position"></param>
        /// <param name="Distance"></param>
        /// <returns></returns>
        public static Position operator +(Position Position, Distance Distance)
        {
            double newLatDecimalDegrees = Position.Latitude.DecimalDegrees;
            double newLongDecimalDegrees = Position.Longitude.DecimalDegrees;
            double newElevation;

            //calculate the exansion of module + direction into geometry
            double latMeters = Distance.OnEarthDistance * Math.Cos(GPSConvertManager.DegreesToRadians(Distance.Direction.Azimuth.DecimalDegrees));
            double longMeters = Distance.OnEarthDistance * Math.Sin(GPSConvertManager.DegreesToRadians(Distance.Direction.Azimuth.DecimalDegrees));

            #region calculate the new lat / long

            //radius * pi = perimeters
            //lengh on peremeters / peremeters = degreeesValue / 360 degree

            newLatDecimalDegrees += (latMeters / (GPSConvertManager.EarthRadiusKms * 2000.0 * Math.PI)) * 360.0;
            newLongDecimalDegrees += (longMeters / (GPSConvertManager.EarthRadiusKms * 2000.0 * Math.PI)) * 360.0;

            #endregion

            #region calculate the new elevation

            newElevation = Distance.Height;

            if (Position.Altitude.HasValue)
            {
                newElevation += Position.Altitude.Value;
            }

            #endregion

            Latitude newLat = new Latitude(newLatDecimalDegrees);
            Longitude newLong = new Longitude(newLongDecimalDegrees);

            return new Position( newLat, newLong , newElevation );
        }



        ///// <summary>
        ///// Subtract two Distance Object (fully Azimuth and Elevation.
        ///// </summary>
        ///// <param name="Distance"></param>
        ///// <returns></returns>
        //public static Distance operator -(Distance Distance)
        //{
        //    Distance temp = new Distance();

        //    temp._absoluteValue = Distance._absoluteValue;
        //    temp._onEarthDistance = Distance._onEarthDistance;

        //    Azimuth newAzimuth = new Azimuth(Distance._direction.Azimuth.DecimalDegrees + 180.0);
        //    Elevation newElevation = new Elevation(-Distance._direction.Elevation.DecimalDegrees);


        //    temp._direction = new Direction(newAzimuth, newElevation);

        //    return temp;
        //}

        #endregion
    }
}
